#ifndef _REPLAN_FSM_H_
#define _REPLAN_FSM_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <vector>
#include <visualization_msgs/Marker.h>

#include <bspline_opt/bspline_optimizer.h>
#include <plan_env/grid_map.h>
#include <geometry_msgs/PoseStamped.h>
#include <plan_manage/planner_manager.h>
#include <traj_utils/planning_visualization.h>
#include <traj_utils/DetectPose.h>
#include <traj_utils/Bspline.h>
#include <traj_utils/YawTraj.h>
#include <traj_utils/MultiBsplines.h>

#include <plan_manage/colors.h>

using std::vector;

namespace ego_planner {

class EGOReplanFSM
{
private:
  /* ---------- flag ---------- */
  enum FSM_EXEC_STATE {
    INIT = 0,
    WAIT_TARGET,
    GEN_NEW_TRAJ,
    REPLAN_TRAJ,
    EXEC_TRAJ,
    EMERGENCY_STOP
  };
  enum TARGET_TYPE {
    MANUAL_TARGET = 1,
    PRESET_TARGET = 2,
    AUTO_FLIGHT = 3
  };

  enum FSM_YAW_STATE {
    YAW_INIT = 0,
    YAW_HOVER_PLAN,
    YAW_HOVER_EXEC,
    YAW_TRAJ_PLAN,
    YAW_TRAJ_EXEC,
  };

  string state_str[8] = {"INIT",
                         "WAIT_TARGET",
                         "GEN_NEW_TRAJ",
                         "REPLAN_TRAJ",
                         "EXEC_TRAJ",
                         "EMERGENCY_STOP"};

  string yaw_state_str[8] = {"YAW_INIT",
                             "YAW_HOVER_PLAN",
                             "YAW_HOVER_EXEC",
                             "YAW_TRAJ_PLAN",
                             "YAW_TRAJ_EXEC"};

  /* planning utils */
  PlanManager::Ptr planner_manager_;
  Visualization::Ptr visualization_;

  /* parameters */
  // 1: use 2D Nav Goal to select goal
  // 2: use global waypoints preset by user
  int target_type_ = 1;
  int current_wp_ = 0;
  double thresh_no_replan_meter_, thresh_replan_time_;
  vector<Eigen::Vector3d> waypoints_;
  int waypoint_num_;
  double planning_horizen_;
  double emergency_time_;
  bool enable_fail_safe_;

  bool isObs_{false};

  bool detect_obs_{false};

  /* planning data */
  FSM_EXEC_STATE exec_state_{INIT};
  FSM_YAW_STATE yaw_state_{YAW_INIT};
  bool have_target_, have_odom_, have_new_target_;
  bool flag_NOT_emergency_ = true;
  int continously_called_times_{0};

  /* odometry state */
  Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_;
  Eigen::Quaterniond odom_orient_;

  Eigen::Vector3d start_pt_, start_vel_, start_acc_, start_yaw_; // start state
  Eigen::Vector3d end_pt_, end_vel_;                             // goal state
  Eigen::Vector3d local_target_pt_, local_target_vel_;           // local target state

  /* ROS utils */
  ros::Timer exec_timer_, safety_timer_, yaw_timer_;
  ros::Subscriber waypoint_sub_, odom_sub_, broadcast_bspline_sub_, detect_sub_;
  ros::Publisher bspline_pub_, broadcast_bspline_pub_, yaw_pub_;

  ros::Publisher pos_list_pub_, cpt_list_pub_, yaw_list_pub_, attract_list_pub_,
      attract_score_list_pub_, debug_cpt_list_pub_, gradient_list_pub_, pk_grad_list_pub_;

  // DEBUG

  /* helper functions */
  bool callReplan(bool flag_use_poly_init, bool flag_randomPolyTraj);
  bool callEmergencyStop(Eigen::Vector3d stop_pos);
  bool planFromGlobalTraj(const int trial_times = 1);
  bool planFromCurrentTraj(const int trial_times = 1);

  void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
  void printFSMExecState();

  void broadcastTrajs();
  void AutoFlight();

  void publishYaw(FSM_YAW_STATE yaw_state);
  void publishTraj(bool isVis = true);

  void getLocalTarget();

  /* ROS Callback functions */
  void execFSMCallback(const ros::TimerEvent &e);
  void checkCollisionCallback(const ros::TimerEvent &e);
  void planYawCallback(const ros::TimerEvent &e);
  void waypointCallback(const nav_msgs::PathConstPtr &msg);
  void triggerCallback(const geometry_msgs::PoseStampedPtr &msg);
  void odometryCallback(const nav_msgs::OdometryConstPtr &msg);
  void BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg);
  void detectCb(const traj_utils::DetectPoseConstPtr &msg);

  /* tool func */
  template <typename T>
  void readParam(ros::NodeHandle &nh,
                 std::string param_name,
                 T &loaded_param)
  {
    const string &node_name = ros::this_node::getName();
    param_name = node_name + "/" + param_name;
    if (!nh.getParam(param_name, loaded_param)) {
      cerr << RED_B << "Failed to load " << param_name
           << ", use default value" << RESET << endl;
    }
    else {
      cout << GREEN_B << "Load " << param_name
           << " success" << RESET << endl;
    }
  }

public:
  EGOReplanFSM(/* args */)
  {
  }
  ~EGOReplanFSM()
  {
  }

  void init(ros::NodeHandle &nh);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace ego_planner

#endif